#include "detect_node.hpp"
#include <rclcpp/parameter_value.hpp>

namespace rm
{

DetectNode::DetectNode(const rclcpp::NodeOptions & options) : Node("detect_node", options)
{
    RCLCPP_INFO(this->get_logger(), "Starting DetectNode!");

    // 初始化图像订阅方
    std::string img_sub_topic_name = this->declare_parameter("image_sub_topic", "image_raw");
    img_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
      img_sub_topic_name, rclcpp::SensorDataQoS(),
      std::bind(&DetectNode::imageCallback, this,
                std::placeholders::_1));


    //相机内参订阅
    std::string cam_info_sub__topic_name = this->declare_parameter("cam_info_sub_topic", "camera_info");
    cam_info_sub_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
        cam_info_sub__topic_name, rclcpp::SensorDataQoS(),
        [this](sensor_msgs::msg::CameraInfo::SharedPtr camera_info) {

            cv::Mat camera_matrix_ = cv::Mat(3, 3, CV_64F, const_cast<double *>(camera_info->k.data())).clone();
            cv::Mat distortion_coefficients_ = cv::Mat(1, 5, CV_64F, const_cast<double *>(camera_info->d.data())).clone();
            
            // 获取相机的长宽（分辨率）
            int camera_width = camera_info->width;   // 图像宽度（像素）
            int camera_height = camera_info->height;  // 图像高度（像素）
           


            // dugbug输出相机内参和分辨率
            RCLCPP_INFO(this->get_logger(), "Camera info received:");
            RCLCPP_INFO(this->get_logger(), "Camera resolution: %d x %d", camera_width, camera_height);
            std::stringstream camera_matrix_ss;
            camera_matrix_ss << camera_matrix_;
            RCLCPP_INFO(this->get_logger(), "Camera matrix: \n%s", camera_matrix_ss.str().c_str());
            std::stringstream dist_coeff_ss;
            dist_coeff_ss << distortion_coefficients_;
            RCLCPP_INFO(this->get_logger(), "Distortion coefficients: \n%s", dist_coeff_ss.str().c_str());

            // 销毁订阅话题
            cam_info_sub_.reset();
    });

    std::string image_pub_topic_name = this->declare_parameter("image_pub_topic", "debug_img");
    image_pub_ = image_transport::create_publisher(this, image_pub_topic_name);


    params_callback_handle_ = this->add_on_set_parameters_callback(
        std::bind(&DetectNode::dynamicParametersCallback, this, std::placeholders::_1));
}

/**
    @brief 回调函数处理图像消息
    @param img_msg 输入的图像消息
*/
void DetectNode::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr img_msg){


    auto img = cv_bridge::toCvShare(img_msg, "bgr8")->image;


    image_pub_.publish(cv_bridge::CvImage(img_msg->header, "bgr8", img).toImageMsg());

    
    double latency = (this->now() - img_msg->header.stamp).seconds();
    RCLCPP_INFO(this->get_logger(),"latency: %f ms", latency*1000);
}


void DetectNode::init_parameter(){


}


/**
    @brief 动态参数回调函数
    @details 处理动态参数的设置，支持多种类型的参数
    @param parameters 输入的参数列表
    @return 返回设置参数结果
*/
rcl_interfaces::msg::SetParametersResult DetectNode::dynamicParametersCallback(
    const std::vector<rclcpp::Parameter> & parameters)
{
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = true;

    for (const auto & param : parameters) {
      const auto & type = param.get_type();
      const auto & name = param.get_name();

      if (type == rclcpp::ParameterType::PARAMETER_DOUBLE) {

/*         if(name == "demo"){
            demo = param.as_double();
        }
        else {
          result.successful = false;
          result.reason = "Unknown integer parameter: " + name;
          continue;
        } */
  
        
      } 
      else if (type == rclcpp::ParameterType::PARAMETER_INTEGER) {

      } 
      else if (type == rclcpp::ParameterType::PARAMETER_BOOL) {

      }
      else if (type == rclcpp::ParameterType::PARAMETER_STRING) {

      }
      else {
        result.successful = false;
        result.reason = "Unsupported parameter type for: " + name;
        continue;
      }
    }

    return result;
}

}

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rm::DetectNode);
